// 批量读取前
#include "my_robot_hardware/zc4_hardware_interface.hpp"

namespace zc4_hardware {

hardware_interface::CallbackReturn Zc4HardwareInterface::on_init(
  const hardware_interface::HardwareInfo & info)
{
  if (hardware_interface::SystemInterface::on_init(info) !=
      hardware_interface::CallbackReturn::SUCCESS)
  {
    return hardware_interface::CallbackReturn::ERROR;
  }

  info_ = info;

  left_motor_id_ = 10;
  right_motor_id_ = 20;
  port_ = "/dev/ttyUSB0";

  driver_ = std::make_shared<XM430Driver>(port_);

  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_configure(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  if (driver_->init() != 0) {
    return hardware_interface::CallbackReturn::ERROR;
  }
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_activate(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  set_state("joint11/velocity", 0.0);
  set_state("joint2/velocity", 0.0);
  set_state("joint11/position", 0.0);
  set_state("joint2/position", 0.0);
  driver_->activateWithVelocityMode(left_motor_id_);
  driver_->activateWithVelocityMode(right_motor_id_);
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::CallbackReturn Zc4HardwareInterface::on_deactivate(
  const rclcpp_lifecycle::State & previous_state)
{
  (void)previous_state;
  driver_->deactivate(left_motor_id_);
  driver_->deactivate(right_motor_id_);
  return hardware_interface::CallbackReturn::SUCCESS;
}

hardware_interface::return_type Zc4HardwareInterface::read(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  (void)time;
  double left_vel = driver_->getVelocityRadianPerSec(left_motor_id_);
  double right_vel = driver_->getVelocityRadianPerSec(right_motor_id_);
  set_state("joint11/velocity", left_vel);
  set_state("joint2/velocity", right_vel);
  set_state("joint11/position",get_state("joint11/position") + left_vel * period.seconds());
  set_state("joint2/position",get_state("joint2/position") + right_vel * period.seconds());
  return hardware_interface::return_type::OK;
}

hardware_interface::return_type Zc4HardwareInterface::write(
  const rclcpp::Time & time, const rclcpp::Duration & period)
{
  (void)time;
  (void)period;
  driver_->setTargetVelocityRadianPerSec(left_motor_id_, get_command("joint11/velocity"));
  driver_->setTargetVelocityRadianPerSec(right_motor_id_, get_command("joint2/velocity"));
  return hardware_interface::return_type::OK;
}

}  // namespace zc4_hardware

#include "pluginlib/class_list_macros.hpp"

PLUGINLIB_EXPORT_CLASS(zc4_hardware::Zc4HardwareInterface, hardware_interface::SystemInterface)